#pragma once
#include "mpc.h"
#include <iostream>
#include <yaml-cpp/yaml.h>
#include <chrono>
#include <string>
#include "vehicle_dyn_factory.hpp"
#include "kf_factory.hpp"
#include <fstream>

using namespace LinearMpc;
namespace auto_ros
{
namespace control
{
class VehicleLMpcBase
{
public:
	VehicleLMpcBase(const std::string dyn_mpc_kf_config_path, const std::string vehicle_lmpc_yaml_name, double rate)
	{
		YAML::Node dyn_mpc_kf_yaml = YAML::LoadFile(dyn_mpc_kf_config_path + vehicle_lmpc_yaml_name);
		std::fstream fout(dyn_mpc_kf_config_path + vehicle_lmpc_yaml_name + ".cor", std::ios::out);
		fout << dyn_mpc_kf_yaml;
		fout.close();
		KfFactory kf_factory;
		if (dyn_mpc_kf_yaml["has_kf"].as<int>() == 1)
		{
			kf_lateral_ptr_ = kf_factory.CreateSharedObject(
				dyn_mpc_kf_yaml["kf_class_name"].as<std::string>(),
				dyn_mpc_kf_config_path + dyn_mpc_kf_yaml["kf_yaml_name"].as<std::string>(), rate);
		}
		VehicleDynFactory dyn_factory;
		std::cout << dyn_mpc_kf_yaml["dyn_type"].as<std::string>() << std::endl;
		vehicle_dyn_ptr_ = dyn_factory.CreateSharedObject(
			dyn_mpc_kf_yaml["dyn_type"].as<std::string>(),
			dyn_mpc_kf_config_path + dyn_mpc_kf_yaml["dyn_yaml_file_name"].as<std::string>());
		mpc_yaml_ = YAML::LoadFile(dyn_mpc_kf_config_path + dyn_mpc_kf_yaml["mpc_yaml_file_name"].as<std::string>());
		rate_ = rate;
		x_ref_ = Eigen::VectorXd::Zero(vehicle_dyn_ptr_->state_dim());	 //resize(dis_dyn_ptr_->state_dim());
		u_ref_ = Eigen::VectorXd::Zero(vehicle_dyn_ptr_->control_dim()); //resize(dis_dyn_ptr_->control_dim());
	}
	virtual void init_mpc() = 0;
	virtual bool step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					  const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
					  double speed, const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, double tire_f_alpha_w,
					  double tire_r_alpha_w) = 0;
	virtual bool step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb, double kappa, double speed) = 0; //q r weighting factor will regulate auto based on yaml file
	virtual bool reset_kf() = 0;
	virtual bool reset_kf(const Eigen::VectorXd &state_esti) = 0;
	std::shared_ptr<Mpc> vehicle_mpc_ptr_;
	std::shared_ptr<VehicleDynBase> vehicle_dyn_ptr_;
	std::shared_ptr<DiscreteLinearSys> dis_dyn_ptr_;
	std::shared_ptr<KfVehicleBase> kf_lateral_ptr_;
	YAML::Node mpc_yaml()
	{
		return mpc_yaml_;
	}
	Eigen::VectorXd x_ref_;
	Eigen::VectorXd u_ref_;
	YAML::Node mpc_yaml_;

protected:
	//YAML::Node vehicle_dyn_config_;

	double rate_;
};

} // namespace control
} // namespace auto_ros